#include <mylib/device/jy61p.h>
#include <mylib/driver/delay.h>
#include <mylib/driver/uart.h>
#include <mylib/utility/debug.h>
#include <stdint.h>
#include <string.h>


// Wit协议：https://wit-motion.yuque.com/wumwnr/ltst03/wegquy
// 数据帧头0x55 类型0x51~0x54 数据低8位 数据高8位 数据低8位 数据高8位 数据低8位 数据高8位 数据低8位
// 数据高8位 SUMCRC（16位） 55 51 9F FD E8 FF A5 07 35 0B 15 加速度 55 52 00 00 00 00 00 00 35 0B E7
// 角速度 55 53 69 21 7D F8 E8 C4 82 46 1B 角度 55 54 00 00 00 00 00 00 00 00 A9 磁场

void jy61p_irq_handler(jy61p_t* jy61p)
{
#if PLATFORM_STM32
    static int32_t len = 0;

    if (jy61p->is_parsing) {
        // 正在解析数据的时候不可以被打断，此时暂时屏蔽接收数据，并且重新开始接收
        len = 0;
    }
    else {
        if (len == 256) {
            len = 0;
            jy61p_data_parse(jy61p);
        }
        uint8_t data         = uart_read_byte(jy61p->uart);
        jy61p->buffer[len++] = data;
    }
    // 接收完成，重新开启接收中断
    // __HAL_UART_ENABLE_IT(jy61p->huart, UART_IT_RXNE);
#elif PLATFORM_MSPM0
    if (DL_UART_getPendingInterrupt(jy61p->huart) == (DL_UART_IIDX)((uint32_t)0x0000000BU)) {
        static int32_t len = 0;

        if (jy61p->is_parsing) {
            // 正在解析数据的时候不可以被打断，此时暂时屏蔽接收数据，并且重新开始接收
            len = 0;
        }
        else {
            if (len == 256) {
                len = 0;
                jy61p_data_parse(jy61p);
            }
            uint8_t data         = DL_UART_receiveData(jy61p->huart);
            jy61p->buffer[len++] = data;
        }
    }
#endif
}

void jy61p_init(jy61p_t* jy61p, uint32_t uart)
{
    memset(jy61p, 0, sizeof(jy61p_t));
    jy61p->uart = uart;
}

uint8_t jy61p_checksum(uint8_t* data, int32_t len)
{
    uint8_t checksum = 0;
    for (int i = 0; i < len; i++) {
        checksum = checksum + data[i];
    }
    return checksum;
}

void jy61p_print_buffer(jy61p_t* jy61p)
{
    debug_print("jy61p buffer: ");
    for (int32_t i = 0; i < 256; i++) {
        debug_print("%02x ", jy61p->buffer[i]);
    }
    debug_print("\n");
}

void jy61p_print_data_frame(uint8_t* data, int32_t len)
{
    debug_print("jy61p data frame: ");
    for (int32_t i = 0; i < len; i++) {
        debug_print("%02x ", data[i]);
    }
    debug_print("\n");
}

// 解析数据
void jy61p_data_parse(jy61p_t* jy61p)
{
    jy61p->is_parsing = true;

    // 查找出以0x55开头的数据
    int32_t start_idx = -1;
    for (int32_t i = 0; i < 256 - 11; i++) {
        if (jy61p->buffer[i] == 0x55) {
            start_idx = i;
            break;
        }
    }
    if (start_idx == -1) {
        DEBUG_INFO("jy61p data parse error");
        // jy61p_print_buffer(jy61p);
        // debug_print("jy61p data parse start_idx: %d\n", start_idx);
        return;
    }

    // 解析数据
    for (int32_t i = start_idx; i < 256; i++) {
        if (jy61p->buffer[i] == 0x55) {
            // checksum
            uint8_t checksum = jy61p_checksum(&jy61p->buffer[i], 10);
            if (checksum != jy61p->buffer[i + 10]) continue;
            if (jy61p->buffer[i + 1] == 0x51) {
                // 加速度数据
                memcpy(&jy61p->acc_frame, &jy61p->buffer[i], 11);
                // jy61p_print_data_frame(jy61p->acc_frame,11);
            }
            else if (jy61p->buffer[i + 1] == 0x52) {
                // 角速度数据
                memcpy(&jy61p->gyro_frame, &jy61p->buffer[i], 11);
                // jy61p_print_data_frame(jy61p->gyro_frame,11);
            }
            else if (jy61p->buffer[i + 1] == 0x53) {
                // 角度数据
                memcpy(&jy61p->angle_frame, &jy61p->buffer[i], 11);
                // jy61p_print_data_frame(jy61p->angle_frame,11);
            }
            i += 10;   // 跳过数据，因为是11个字节
        }
    }

    jy61p->is_parsing = false;
}

// 合成字节到短整型
// 这里必须是先转为int16_t，再合成，合成完了重新转int16_t
// 参考https://wit-motion.yuque.com/wumwnr/ltst03/vl3tpy?#tBLhV
static inline int16_t combine_byte_to_short(uint8_t high, uint8_t low)
{
    return ((int16_t)high << 8) | low;
}

// 获取加速度
void jy61p_get_acc(jy61p_t* jy61p, float* acc_x, float* acc_y, float* acc_z)
{
    // 解析数据
    // fAcc[i] = sReg[AX+i] / 32768.0f * 16.0f * g; // g为重力加速度，这里假设为9.8m/s^2
    // 重力加速度G
    static const float gravitational_acceleration = 9.8f;

    uint8_t low, high;
    low         = jy61p->acc_frame[2];
    high        = jy61p->acc_frame[3];
    int16_t tmp = combine_byte_to_short(high, low);
    *acc_x      = (float)tmp / 32768.0f * 16.0f * gravitational_acceleration;

    low    = jy61p->acc_frame[4];
    high   = jy61p->acc_frame[5];
    tmp    = combine_byte_to_short(high, low);
    *acc_y = (float)tmp / 32768.0f * 16.0f * gravitational_acceleration;

    low    = jy61p->acc_frame[6];
    high   = jy61p->acc_frame[7];
    tmp    = combine_byte_to_short(high, low);
    *acc_z = (float)tmp / 32768.0f * 16.0f * gravitational_acceleration;

    // low = jy61p->acc_frame[8];
    // high = jy61p->acc_frame[9];
    // tmp = (int16_t)(high << 8) | (int16_t)low;
    // *acc_temperature = (float)tmp / 100.0f;
}

// 获取角速度
void jy61p_get_gyro(jy61p_t* jy61p, float* gyro_x, float* gyro_y, float* gyro_z)
{
    // 解析数据
    // fGyro[i] = sReg[GX+i] / 32768.0f * 2000.0f;

    uint8_t low, high;
    low         = jy61p->gyro_frame[2];
    high        = jy61p->gyro_frame[3];
    int16_t tmp = combine_byte_to_short(high, low);
    *gyro_x     = (float)tmp / 32768.0f * 2000.0f;

    low     = jy61p->gyro_frame[4];
    high    = jy61p->gyro_frame[5];
    tmp     = combine_byte_to_short(high, low);
    *gyro_y = (float)tmp / 32768.0f * 2000.0f;

    low     = jy61p->gyro_frame[6];
    high    = jy61p->gyro_frame[7];
    tmp     = combine_byte_to_short(high, low);
    *gyro_z = (float)tmp / 32768.0f * 2000.0f;
}

// 获取角度
void jy61p_get_angle(jy61p_t* jy61p, float* roll, float* pitch, float* yaw)
{
    // 解析数据
    // fAngle[i] = sReg[Roll+i] / 32768.0f * 180.0f;

    // jy61p_print_data_frame(jy61p->angle_frame, 11);

    uint8_t low, high;
    low         = jy61p->angle_frame[2];
    high        = jy61p->angle_frame[3];
    int16_t tmp = combine_byte_to_short(high, low);
    *roll       = (float)tmp / 32768.0f * 180.0f;

    low    = jy61p->angle_frame[4];
    high   = jy61p->angle_frame[5];
    tmp    = combine_byte_to_short(high, low);
    *pitch = (float)tmp / 32768.0f * 180.0f;

    low  = jy61p->angle_frame[6];
    high = jy61p->angle_frame[7];
    tmp  = combine_byte_to_short(high, low);
    *yaw = (float)tmp / 32768.0f * 180.0f;
}

static uint8_t JY61P_ULOCK_CMD[5] = {0xFF, 0xAA, 0x69, 0x88, 0xB5};   // 解锁
static uint8_t JY61P_SAVE_CMD[5]  = {0xFF, 0xAA, 0x00, 0x00, 0x00};   // 保存
static uint8_t JY61P_ACC_CMD[5]   = {0xFF, 0xAA, 0x01, 0x01, 0x00};   // 加速度校准
static uint8_t JY61P_XY0_CMD[5]   = {0xFF, 0xAA, 0x01, 0x08, 0x00};   // XY轴归零
static uint8_t JY61P_Z0_CMD[5]    = {0xFF, 0xAA, 0x01, 0x04, 0x00};   // Z轴归零

void jy61p_send_cmd(jy61p_t* jy61p, uint8_t* cmd, int32_t len)
{
    uart_send(jy61p->uart, cmd, len);
}

// 校准加速度器
void jy61p_acc_calibration(jy61p_t* jy61p)
{
    // 发送解锁
    jy61p_send_cmd(jy61p, JY61P_ULOCK_CMD, sizeof(JY61P_ULOCK_CMD));
    // delay 200ms
    delay_ms_block(200);
    // 发送加速度校准
    jy61p_send_cmd(jy61p, JY61P_ACC_CMD, sizeof(JY61P_ACC_CMD));
    // delay 5000ms
    delay_ms_block(5000);
    // 发送保存
    jy61p_send_cmd(jy61p, JY61P_SAVE_CMD, sizeof(JY61P_SAVE_CMD));
}

// xy轴置零
void jy61p_zero_xy(jy61p_t* jy61p)
{
    // 发送解锁
    jy61p_send_cmd(jy61p, JY61P_ULOCK_CMD, sizeof(JY61P_ULOCK_CMD));
    // delay 200ms
    delay_ms_block(200);
    // 发送加速度校准
    jy61p_send_cmd(jy61p, JY61P_XY0_CMD, sizeof(JY61P_XY0_CMD));
    // delay 3000ms
    delay_ms_block(3000);
    // 发送保存
    jy61p_send_cmd(jy61p, JY61P_SAVE_CMD, sizeof(JY61P_SAVE_CMD));
}

// z轴置零
void jy61p_zero_yaw(jy61p_t* jy61p)
{
    // 参考：https://blog.csdn.net/m0_52011717/article/details/138538530
    // 发送解锁
    jy61p_send_cmd(jy61p, JY61P_ULOCK_CMD, sizeof(JY61P_ULOCK_CMD));
    // delay 200ms
    delay_ms_block(200);
    // 发送加速度校准
    jy61p_send_cmd(jy61p, JY61P_Z0_CMD, sizeof(JY61P_Z0_CMD));
    // delay 3000ms
    delay_ms_block(3000);
    // 发送保存
    jy61p_send_cmd(jy61p, JY61P_SAVE_CMD, sizeof(JY61P_SAVE_CMD));
}

///////////////////////////////////////////////////////////////////////////////

// 测试代码

#if 0

jy61p_t            jy61p;
extern UART_HandleTypeDef huart1;

void serial_send_byte(uint8_t byte)
{
    HAL_UART_Transmit(&huart1, &byte, 1, 1000);
}

void delay_ms(int32_t ms)
{
    HAL_Delay(ms);
}

void jy61p_test(void)
{
    // 初始化串口，这里假设已经初始化好了huart1
    jy61p_init(&jy61p, &huart1, serial_send_byte, delay_ms);
    // xy轴置零
    jy61p_zero_xy(&jy61p);
    // z轴置零
    jy61p_zero_yaw(&jy61p);

    while (1) {
        // 读取数据
        // 加速度数据
        float ax, ay, az;
        jy61p_get_acc(&jy61p, &ax, &ay, &az);
        DEBUG_INFO("ax:%.2f ay:%.2f az:%.2f", ax, ay, az);
        // 角速度数据
        float gx, gy, gz;
        jy61p_get_gyro(&jy61p, &gx, &gy, &gz);
        DEBUG_INFO("gx:%.2f gy:%.2f gz:%.2f", gx, gy, gz);
        // 角度数据
        float roll, pitch, yaw;
        jy61p_get_angle(&jy61p, &roll, &pitch, &yaw);
        DEBUG_INFO("roll:%.2f pitch:%.2f yaw:%.2f", roll, pitch, yaw);
    }
}

void USART1_IRQHandler(void)
{
    HAL_UART_IRQHandler(&huart1);
    jy61p_irq_handler(&jy61p);
}

#endif

////////////////////////////////////////////////////////////////////////////////
